#include <peripheral/ports.h>
#include <peripheral/uart.h>
#include <peripheral/system.h>
#include <peripheral/int.h>
#include <peripheral/timer.h>
#include <stdio.h>
#include <stdlib.h>
#include "tick.h"
#include "task.h"
#include "readyqueue.h"

#pragma config FNOSC = PRIPLL
#pragma config FPLLMUL = MUL_20
#pragma config FPLLIDIV = DIV_2
#pragma config FPBDIV = DIV_2
#pragma config FPLLODIV = DIV_1
#pragma config POSCMOD = HS
#pragma config FWDTEN = OFF

void writeS (char *s);
void writeC (char c);
void task1 (void *parameters);
void task2 (void *parameters);

volatile int timer2flag = 0;

int main (void)
{
	int i;
	unsigned int pbclock;
	struct task *t;
	char buf [16];
	unsigned int task1args [2];
	unsigned int task2args;
	unsigned int task3args [2];
	unsigned int task4args [2];
	unsigned int task5args [2];
	pbclock = SYSTEMConfigPerformance (80000000);

	PORTSetPinsDigitalOut (IOPORT_B, BIT_10 | BIT_11 | BIT_12 | BIT_13);
	PORTClearBits (IOPORT_B, BIT_10 | BIT_11 | BIT_12 | BIT_13);

	//OpenTimer2 (T2_ON | T2_IDLE_CON | T2_PS_1_256, pbclock / 2560); // Runs 10 times per second
	OpenTimer2 (T2_ON | T2_IDLE_CON | T2_PS_1_64, pbclock / 64000); // Runs 1000 times per second
	ConfigIntTimer2 (T2_INT_ON | T2_INT_PRIOR_1);

	// UART1
	PORTSetPinsDigitalOut (IOPORT_F, BIT_8); // TX
	PORTRead (IOPORT_F);
	UARTSetDataRate (UART1, pbclock, 9600);
	UARTSetLineControl (UART1, UART_DATA_SIZE_8_BITS | UART_PARITY_EVEN);
	UARTEnable (UART1, UART_ENABLE | UART_PERIPHERAL | UART_TX);

	task1args [0] = BIT_10;
	task1args [1] = 2;

	task2args = 1;

	task3args [0] = BIT_13;
	task3args [1] = 4;

	task4args [0] = BIT_12;
	task4args [1] = 8;

	task5args [0] = BIT_11;
	task5args [1] = 16;

	taskCreate (task1, 256, &task1args);
	taskCreate (task1, 256, &task3args);
	taskCreate (task1, 256, &task4args);
	taskCreate (task1, 256, &task5args);
	taskCreate (task2, 256, &task2args);

	startScheduler ();

}

void writeS (char *s)
{
	if (*s)
	{
		writeC (*s);
		writeS (++ s);
	}
}

void writeC (char c)
{
	while (! UARTTransmitterIsReady (UART1))
	{
		;
	}
	UARTSendDataByte (UART1, c);
	while (! UARTTransmissionHasCompleted (UART1))
	{
		;
	}
}

void __ISR (_TIMER_2_VECTOR, ipl1) timer2 (void)
{
	volatile static unsigned int counter = 0;
	counter ++;
	
	if (counter % 100 == 0)
	{
		semaphoreGive (1);
	}
	if (counter % (rand () % 1000 + 1) == 0)
	{
		semaphoreGive (2);
	}
	if (counter % (rand () % 1000 + 1) == 0)
	{
		semaphoreGive (4);
	}
	if (counter % (rand () % 1000 + 1) == 0)
	{
		semaphoreGive (8);
	}
	if (counter % (rand () % 1000 + 1) == 0)
	{
		semaphoreGive (16);
	}
	mT2ClearIntFlag ();
}

void task1 (void *parameters)
{
	unsigned int led;
	unsigned int sem;
	led = ((unsigned int *) parameters) [0];
	sem = ((unsigned int *) parameters) [1];
	while (1)
	{
		semaphoreTake (sem);
		//taskDelay (10000);
		PORTToggleBits (IOPORT_B, led);
	}
}

void task2 (void *parameters)
{
	unsigned int sem;
	int t;
	char s [24];
	char home [] = {0x1B, '[', '0', ';', '0', 'H', 0};
	sem = *((unsigned int *) parameters);
	while (1)
	{
		semaphoreTake (sem);
		t = getTickCount ();
		sprintf (s, "%s%d.%d", home, t / 1000, (t % 1000) / 100);
		writeS (s);
	}
}

